#ifndef KALMANFILTER_HPP
#define	KALMANFILTER_HPP

#include "visualization.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/tracking.hpp>

#include <string.h>
#include <cstdio>

template<typename T>
class Kalman {
public:

    Kalman(int DM, int MM, int CM,
            T* A, T* B, T* H, T* Q, T* R,
            T* error_covariance, T* state);


    Kalman();

    void updateKalman(INFO_ROBOT& info);

private:

    cv::KalmanFilter kalmanMark1;
    cv::KalmanFilter kalmanMark2;

    cv::Mat_< T > measurement1;
    cv::Mat predict1, estimated1;
    cv::Mat_< T > measurement2;
    cv::Mat predict2, estimated2;

};

template <typename T>
Kalman< T >::Kalman() {

    int DM = 8;
    int MM = 4;
    int CM = 0;

    float deltaTime = 0.005;
    float qX = 0.05;
    float qY = 0.05;
    //    float qTheta = 0.05;
    float rX = 0.1;
    float rY = 0.1;
    //    float rTheta = 0.1;

    float Q[] = {
        qX, 0, 0, 0,
        0, qY, 0, 0,
        0, 0, qX, 0,
        0, 0, 0, qX
    };

    float R[] = {
        rX, 0,
        0, rY
    };

    float A[] = {
        1, 0, deltaTime, 0,
        0, 1, 0, deltaTime,
        0, 0, 1, 0,
        0, 0, 0, 1
    };

    float H[] = {
        1, 0, 0, 0,
        0, 1, 0, 0
    };

    float error_covariance[] = {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1
    };

    float state[] = {0, 0, 1, 1};

    this->kalmanMark1.init(DM, MM, CM);
    this->kalmanMark2.init(DM, MM, CM);

    this->kalmanMark1.transitionMatrix = (cv::Mat_< T > (DM, DM, A));
    //    if (CM)
    //        this->kalmanMark1.controlMatrix = (cv::Mat_< T > (CM, CM, B));

    this->kalmanMark2.transitionMatrix = (cv::Mat_< T > (DM, DM, A));
    //    if (CM)
    //        this->kalmanMark2.controlMatrix = (cv::Mat_< T > (CM, CM, B));

    this->kalmanMark1.measurementMatrix = (cv::Mat_< T > (MM, DM, H));
    this->kalmanMark1.processNoiseCov = (cv::Mat_< T > (DM, DM, Q));
    this->kalmanMark1.measurementNoiseCov = (cv::Mat_< T > (MM, MM, R));

    this->kalmanMark2.measurementMatrix = (cv::Mat_< T > (MM, DM, H));
    this->kalmanMark2.processNoiseCov = (cv::Mat_< T > (DM, DM, Q));
    this->kalmanMark2.measurementNoiseCov = (cv::Mat_< T > (MM, MM, R));

    this->kalmanMark1.statePre = (cv::Mat_< T > (DM, 1, state));
    this->kalmanMark1.errorCovPost = (cv::Mat_< T > (DM, DM, error_covariance));

    this->kalmanMark2.statePre = (cv::Mat_< T > (DM, 1, state));
    this->kalmanMark2.errorCovPost = (cv::Mat_< T > (DM, DM, error_covariance));

    this->measurement1 = (cv::Mat_< T > (MM, 1));
    this->measurement1.setTo(cv::Scalar(0));

    this->measurement2 = (cv::Mat_< T > (MM, 1));
    this->measurement2.setTo(cv::Scalar(0));
}

template <typename T>
Kalman< T >::Kalman(int DM, int MM, int CM,
        T* A, T* B, T* H, T* Q,
        T* R, T* error_covariance, T* state) {

    this->kalmanMark1.init(DM, MM, CM);
    this->kalmanMark2.init(DM, MM, CM);

    this->kalmanMark1.transitionMatrix = (cv::Mat_< T > (DM, DM, A));
    if (CM)
        this->kalmanMark1.controlMatrix = (cv::Mat_< T > (CM, CM, B));

    this->kalmanMark2.transitionMatrix = (cv::Mat_< T > (DM, DM, A));
    if (CM)
        this->kalmanMark2.controlMatrix = (cv::Mat_< T > (CM, CM, B));

    this->kalmanMark1.measurementMatrix = (cv::Mat_< T > (MM, DM, H));
    this->kalmanMark1.processNoiseCov = (cv::Mat_< T > (DM, DM, Q));
    this->kalmanMark1.measurementNoiseCov = (cv::Mat_< T > (MM, MM, R));

    this->kalmanMark2.measurementMatrix = (cv::Mat_< T > (MM, DM, H));
    this->kalmanMark2.processNoiseCov = (cv::Mat_< T > (DM, DM, Q));
    this->kalmanMark2.measurementNoiseCov = (cv::Mat_< T > (MM, MM, R));

    this->kalmanMark1.statePre = (cv::Mat_< T > (DM, 1) << 0, 0, 0, 0, 1, 1, 1, 1);
    this->kalmanMark1.errorCovPost = (cv::Mat_< T > (DM, DM, error_covariance));

    this->kalmanMark2.statePre = (cv::Mat_< T > (DM, 1) << 0, 0, 0, 0, 1, 1, 1, 1);
    this->kalmanMark2.errorCovPost = (cv::Mat_< T > (DM, DM, error_covariance));

    this->measurement1 = (cv::Mat_< T > (MM, 1));
    this->measurement1.setTo(cv::Scalar(0));

    this->measurement2 = (cv::Mat_< T > (MM, 1));
    this->measurement2.setTo(cv::Scalar(0));
}

template <typename T>
void Kalman< T >::updateKalman(INFO_ROBOT& info) {

    this->predict1 = this->kalmanMark1.predict();
    this->predict2 = this->kalmanMark2.predict();
    printf("Antes 1: %d %d\n", info.pixelX1, info.pixelY1);
    printf("Antes 2: %d %d\n", info.pixelX2, info.pixelY2);

    if (info.find) {

        this->measurement1(0) = (float) info.pixelX1;
        this->measurement1(1) = (float) info.pixelY1;

        this->measurement2(0) = (float) info.pixelX2;
        this->measurement2(1) = (float) info.pixelY2;

        //por enquanto o erro continua, agora foi solucionando (04/08/2012)
        this->estimated1 = this->kalmanMark1.correct(this->measurement1);
        this->estimated2 = this->kalmanMark2.correct(this->measurement2);

    } else {

        this->kalmanMark1.statePost = this->kalmanMark1.statePre;
        this->estimated1 = this->predict1;

        this->kalmanMark2.statePost = this->kalmanMark2.statePre;
        this->estimated2 = this->predict2;
    }

    info.find = true;
    info.pixelX1 = (int) kalmanMark1.statePost.at<T > (0);
    info.pixelY1 = (int) kalmanMark1.statePost.at<T > (1);

    info.pixelX2 = (int) kalmanMark2.statePost.at<T > (0);
    info.pixelY2 = (int) kalmanMark2.statePost.at<T > (1);

    printf("Depois 1: %d %d\n", info.pixelX1, info.pixelY1);
    printf("Dpois 2: %d %d\n", info.pixelX2, info.pixelY2);
}

#endif	/* KALMANFILTER_HPP */

